/**
 *******************************************************************************
 * @file      : quaternion_ekf.h
 * @brief     :
 * @history   :
 *  Version     Date            Author          Note
 *  V0.9.0      yyyy-mm-dd      <author>        1. <note>
 *******************************************************************************
 * @attention :
 *******************************************************************************
 *  Copyright (c) 2023 Reborn Team, USTB.
 *  All Rights Reserved.
 *******************************************************************************
 */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __QUAT_EKF_H_
#define __QUAT_EKF_H_

#ifdef __cplusplus

/* Includes ------------------------------------------------------------------*/
#include "kalman_filter.h"
/* Exported macro ------------------------------------------------------------*/
/* boolean type definitions */
#ifndef TRUE
#define TRUE 1 /**< boolean true  */
#endif

#ifndef FALSE
#define FALSE 0 /**< boolean fails */
#endif

/* Exported constants --------------------------------------------------------*/
/* Exported types ------------------------------------------------------------*/
typedef struct
{
    uint8_t Initialized;
    KalmanFilter_t IMU_QuaternionEKF;
    uint8_t ConvergeFlag;
    uint8_t StableFlag;
    uint64_t ErrorCount;
    uint64_t UpdateCount;

    float q[4];         // 四元数估计值
    float GyroBias[3];  // 陀螺仪零偏估计值

    float Gyro[3];
    float Accel[3];

    float OrientationCosine[3];

    float accLPFcoef;
    float gyro_norm;
    float accl_norm;
    float AdaptiveGainScale;

    float Roll;
    float Pitch;
    float Yaw;

    float YawTotalAngle;

    float Q1;  // 四元数更新过程噪声
    float Q2;  // 陀螺仪零偏过程噪声
    float R;   // 加速度计量测噪声

    float dt;  // 姿态更新周期
    mat ChiSquare;
    float ChiSquare_Data[1];       // 卡方检验检测函数
    float ChiSquareTestThreshold;  // 卡方检验阈值
    float lambda;                  // 渐消因子

    int16_t YawRoundCount;

    float YawAngleLast;
} QEKF_INS_t;

/* Exported variables --------------------------------------------------------*/
extern QEKF_INS_t QEKF_INS;
extern float chiSquare;
extern float ChiSquareTestThreshold;

/* Exported function prototypes ----------------------------------------------*/
void IMU_QuaternionEKF_Init(float* init_quaternion, float process_noise1, float process_noise2, float measure_noise, float lambda, float lpf);
void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, float az, float dt);

#endif

#endif /* __QUAT_EKF_H_ */
